package de.fhwf.mixedreality.client2007;

import de.fhwf.mixedreality.client.IMovements;
import de.fhwf.mixedreality.client.Vector;

public class Movements implements IMovements {
	private PositionsStorage mPositions;
	private CommandInterface mCmd;

	public Movements(PositionsStorage positions, CommandInterface command) {
		mPositions = positions;
		mCmd = command;
	}

	/**
	 * This makes the robot go toward a vector, turning and making curves when
	 * necessary.
	 */
	public void goTo(Vector target) {
		// Adjust the angle to be between -180 and 180 degrees.
		target.boundAngle();

		// If the angle error is bigger than 90 degrees let's just turn
		if (target.getAngle() > Math.PI / 2) {
			turnRight();
		} else if (target.getAngle() < -Math.PI / 2) {
			turnLeft();
		} else if (target.getLength() > 300) { // Otherwise let's check the distance
			// We are still far, let's not care much about being completely aligned.
			if (target.getAngle() > (20.0 * Math.PI / 180.0)) {
				curveRight();
			} else if (target.getAngle() < (-20.0 * Math.PI / 180.0)) {
				curveLeft();
			} else if (target.getLength() > 600) {
				fastForward();
			} else {
				goStraight(3);
			}
		} else if (target.getLength() > 80) {
			// Okay we are closer, let's narrow the angle
			if (target.getAngle() > (10.0 * Math.PI / 180.0)) {
				curveRight();
			} else if (target.getAngle() < (-10.0 * Math.PI / 180.0)) {
				curveLeft();
			} else {
				// Angle seems OK, let's keep going straight ahead
				goStraight(1);
			}
		} else if (target.getLength() > 20) {
			// Okay we are closer, let's narrow the angle
			if (target.getAngle() > (5.0 * Math.PI / 180.0)) {
				setVelocity(1, 0);
			} else if (target.getAngle() < (-5.0 * Math.PI / 180.0)) {
				setVelocity(0, 1);
			} else {
				// Angle seems OK, let's keep going straight ahead
				goStraight(1);
			}
		} else {
			// Good enough! We stop here.
			stop();
		}
	}

	/**
	 * Set the velocity of the wheels. Any combination of the follwing velocities
	 * is possible: -3, -2, -1, 0, 1, 2, 3
	 */
	public void setVelocity(int left, int right) {
		mCmd.setVelocity(left, right);
	}

	/**
	 * Directly set the velocity of the wheels.
	 * The allowed range for the values depends on the robot version used:
	 * 2007:   -4 .. 0 ..   4
	 */
	public void setRawSpeed(int left, int right) {
		mCmd.setVelocity(left, right);
	}

	/** Send raw command string (four characters) */
	public void rawCommand(String cmd) {
		mCmd.setRaw(cmd);
	}

	/**
	 * Makes the robot go straight with same velocity for both wheels. The allowed
	 * velocities are: -3, -2, -1, 0, 1, 2, 3
	 */
	public void goStraight(int speed) {
		setVelocity(speed, speed);
	}

	/** Stops the robot */
	public void stop() {
		setVelocity(0, 0);
	}

	/** Makes the robot turn left */
	public void turnLeft() {
		setVelocity(-3, 3);
	}

	/** Makes the robot turn right */
	public void turnRight() {
		setVelocity(3, -3);
	}

	/** Makes the robot go in a curve with a trajectory of an arc to the left */
	public void curveLeft() {
		setVelocity(1, 3);
	}

	/** Makes the robot go in a curve with a trajectory of an arc to the right */
	public void curveRight() {
		setVelocity(3, 1);
	}

	/**
	 * This makes the robot go toward a vector, turning and making curves when
	 * necessary.
	 */
	public void chase(Vector target) {
		goTo(target);
	}

	/**
	 * This is the oposite of goTo, making the robot escape away from the given
	 * vector.
	 */
	public void escapeFrom(Vector enemy) {
		goTo(new Vector(enemy.getAngle() + Math.PI, enemy.getLength()));
	}

	/**
	 * This function makes the robot kick the ball. The angle gives the direction
	 * of the kick, the strength the force applied. Strenght 0 - 1; Angle Radiant
	 * -pi - +pi
	 */
	public void kick(double angle, double strength) {
		mCmd.setKick(angle, strength);
	}

	/** Tells whether the robot can kick or not. */
	public boolean canKick() {
		Vector t = mPositions.ball();
		if (t.getLength() < 80) {
			return true;
		} else {
			return false;
		}
	}

	public void fastBackward() {
	 rawCommand("BNBN");
	}

	public void fastForward() {
	 rawCommand("FNFN");
	}

	public void serverGoTo(Vector target) {
	 // Not supported by 2007 server
	}
}